// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------
// Modification history:
// feature: authentication-free shared key and token derivation between clients
// feature: change E2E packet format
// feature: DServer support simple entity
// feature: secure discovery server and client
// feature: support auth token
// feature: develop dispatcher
// feature: Unified Transport Abstraction Layer Unified Transport Abstraction Layer
// feature: interval report metrics
// feature: E2E New Requirement Development
// feature: Development of new requirements for E2E functionality
// ------------------------------------------------------------------

#include <edds/rtps/messages/RTPSMessageGroup.h>

#include <edds/rtps/messages/RTPSMessageCreator.h>
#include <edds/rtps/reader/RTPSReader.h>
#include <edds/rtps/writer/RTPSWriter.h>
#include <rtps/participant/RTPSParticipantImpl.h>
#include <ecdr/exceptions/BadParamException.h>

#include <algorithm>

#include <elog/Log.hpp>
#include <rtps/messages/RTPSGapBuilder.hpp>
#include <rtps/messages/RTPSMessageGroup_t.hpp>

#include <deps/common/SystemInfo.hpp>
#include <edds/rtps/common/VendorId_t.hpp>
#include <edds/dds/core/policy/ParameterList.hpp>

#if !defined(_WIN32)
#include <tracepoint/TracePointManager.hpp>
#endif
namespace evbs {
namespace ertps {
namespace rtps {

/**
 * An InlineQosWriter that puts the inline_qos of a CacheChange_t into a CDRMessage_t.
 */
class CacheChangeInlineQoSWriter final : public InlineQosWriter {
    const CacheChange_t& change_;

 public:
    explicit CacheChangeInlineQoSWriter(const CacheChange_t& change) : change_(change) {}

    bool writeQosToCDRMessage(CDRMessage_t* msg) final {
        return CDRMessage::addData(msg, change_.inline_qos.data, change_.inline_qos.length);
    }
};

static inline bool data_exceeds_limitation(uint32_t size_to_add, uint32_t limitation, uint32_t total_sent,
                                           uint32_t pending_to_send) {
    return
        // Limitation has been set and
        (0U < limitation) &&
        //   either limitation has already been reached
        ((limitation <= (total_sent + pending_to_send)) ||
         //   or adding size_to_add will exceed limitation
         (size_to_add > (limitation - (total_sent + pending_to_send))));
}

bool sort_changes_group(CacheChange_t* c1, CacheChange_t* c2) {
    return (c1->sequenceNumber < c2->sequenceNumber);
}

bool sort_SeqNum(const SequenceNumber_t& s1, const SequenceNumber_t& s2) {
    return (s1 < s2);
}

typedef std::pair<SequenceNumber_t, SequenceNumberSet_t> pair_T;

bool compare_remote_participants(const std::vector<GUID_t>& remote_participants1,
                                 const std::vector<GuidPrefix_t>& remote_participants2) {
    if (remote_participants1.size() == remote_participants2.size()) {
        for (auto& participant : remote_participants1) {
            if (std::find(remote_participants2.begin(), remote_participants2.end(), participant.guidPrefix) ==
                remote_participants2.end()) {
                return false;
            }
        }

        return true;
    }

    return false;
}

void get_participant_from_endpoint(const GUID_t& endpoint, std::vector<GuidPrefix_t>& participants) {
    if (std::find(participants.begin(), participants.end(), endpoint.guidPrefix) == participants.end()) {
        participants.push_back(endpoint.guidPrefix);
    }
}

void get_participants_from_endpoints(const std::vector<GUID_t>& endpoints, std::vector<GuidPrefix_t>& participants) {
    participants.clear();

    for (const GUID_t& endpoint : endpoints) {
        if (std::find(participants.begin(), participants.end(), endpoint.guidPrefix) == participants.end()) {
            participants.push_back(endpoint.guidPrefix);
        }
    }
}

const EntityId_t& get_entity_id(const std::vector<GUID_t>& endpoints) {
    if (endpoints.size() == 0U) {
        return c_EntityId_Unknown;
    }

    const EntityId_t& entityid = endpoints.at(0U).entityId;

    for (auto it = endpoints.begin() + 1; it != endpoints.end(); ++it) {
        if (entityid != it->entityId) {
            return c_EntityId_Unknown;
        }
    }

    return entityid;
}

RTPSMessageGroup::RTPSMessageGroup(RTPSParticipantImpl* participant, bool internal_buffer)
    : participant_(participant),
      send_buffer_(!internal_buffer ? participant->get_send_buffer() : nullptr),
      internal_buffer_(internal_buffer) {
    // Avoid warning when neither SECURITY nor DEBUG is used

    if (participant == nullptr) {
        return;
    }

    if (internal_buffer) {
        const GuidPrefix_t& guid_prefix = participant->getGuid().guidPrefix;
        constexpr size_t align_size = sizeof(octet*) - 1;
        uint32_t payload_size = participant->getMaxMessageSize();
        // assert(payload_size > 0u);
        if (payload_size == 0u) {
            elogError(RTPS_MSG_OUT, RetCode_t::RETCODE_BAD_PARAMETER, "payload_size is 0.");
            return;
        }
        payload_size = (payload_size + align_size) & ~align_size;
        send_buffer_ = new RTPSMessageGroup_t(
#if HAVE_SECURITY
            participant->is_secure(),
#endif  // if HAVE_SECURITY
            payload_size, guid_prefix);
    }

    full_msg_ = &(send_buffer_->rtpsmsg_fullmsg_);

    // Init RTPS message.
    reset_to_header();

#if HAVE_SECURITY
    if (participant->is_secure()) {
        encrypt_msg_ = &(send_buffer_->rtpsmsg_encrypt_);
        (void)CDRMessage::initCDRMsg(encrypt_msg_);
    }
#endif  // if HAVE_SECURITY
}

RTPSMessageGroup::RTPSMessageGroup(RTPSParticipantImpl* participant, Endpoint* endpoint,
                                   RTPSMessageSenderInterface* msg_sender)
    : RTPSMessageGroup(participant) {
    assert(endpoint);

    endpoint_ = endpoint;
    sender_ = msg_sender;
    max_blocking_time_is_set_ = false;
}

RTPSMessageGroup::RTPSMessageGroup(RTPSParticipantImpl* participant, Endpoint* endpoint,
                                   RTPSMessageSenderInterface* msg_sender, bool add_info_src,
                                   GuidPrefix_t src_guid_prefix)
    : RTPSMessageGroup(participant) {
    assert(endpoint);

    endpoint_ = endpoint;
    sender_ = msg_sender;
    max_blocking_time_is_set_ = false;
    add_info_src_ = add_info_src;
    src_guid_prefix_ = src_guid_prefix;
}

RTPSMessageGroup::RTPSMessageGroup(RTPSParticipantImpl* participant, Endpoint* endpoint,
                                   RTPSMessageSenderInterface* msg_sender, bool add_info_src,
                                   GuidPrefix_t src_guid_prefix, DomainId_t domain_id)
    : RTPSMessageGroup(participant) {
    assert(endpoint);

    endpoint_ = endpoint;
    sender_ = msg_sender;
    max_blocking_time_is_set_ = false;
    add_info_src_ = add_info_src;
    src_guid_prefix_ = src_guid_prefix;
    domain_id_ = domain_id;
}

RTPSMessageGroup::~RTPSMessageGroup() {
    try {
        send();
    } catch (...) {
        // if (!internal_buffer_) {
        //     participant_->return_send_buffer(std::move(send_buffer_));
        // }
        // throw;
        elogError(RTPS_MSG_OUT, RetCode_t::RETCODE_TIMEOUT,
                  "throw except: evbs::ertps::rtps::RTPSMessageGroup::timeout");
    }

    if (!internal_buffer_) {
        participant_->return_send_buffer(send_buffer_);
    } else {
        delete send_buffer_;
    }
}

inline void RTPSMessageGroup::reset_to_header() {
    (void)CDRMessage::initCDRMsg(full_msg_);
    full_msg_->pos = RTPSMESSAGE_HEADER_SIZE;
    full_msg_->last_pos = RTPSMESSAGE_HEADER_SIZE;
    full_msg_->length = RTPSMESSAGE_HEADER_SIZE;
}

inline void RTPSMessageGroup::flush() {
    try {
        send();
    } catch (const evbs::ertps::rtps::RTPSMessageGroup::timeout&) {
        elogErrorT(RTPS_MSG_OUT, RetCode_t::RETCODE_TIMEOUT, "throw except RTPSMessageGroup::timeout");
    }

    reset_to_header();
}

inline void RTPSMessageGroup::send() {
    if (endpoint_ && sender_) {
        CDRMessage_t* msgToSend = full_msg_;

        if (full_msg_->length > RTPSMESSAGE_HEADER_SIZE) {
            std::lock_guard<RTPSMessageSenderInterface> lock(*sender_);

#if HAVE_SECURITY
            // TODO(Ricardo) Control message size if it will be encrypted.
            if (participant_->security_attributes().is_rtps_protected && endpoint_->supports_rtps_protection()) {
                bool ret = CDRMessage::initCDRMsg(encrypt_msg_);
                if (ret) {
                    full_msg_->pos = RTPSMESSAGE_HEADER_SIZE;
                    encrypt_msg_->pos = RTPSMESSAGE_HEADER_SIZE;
                    encrypt_msg_->length = RTPSMESSAGE_HEADER_SIZE;
                    (void)memcpy(encrypt_msg_->buffer, full_msg_->buffer, RTPSMESSAGE_HEADER_SIZE);
                } else {
                    elogError(RTPS_WRITER, RetCode_t::RETCODE_ERROR, "Error initCDRMsg.");
                    return;
                }

                if (!participant_->security_manager().encode_rtps_message(*full_msg_, *encrypt_msg_,
                                                                          sender_->remote_participants())) {
                    elogError(RTPS_WRITER, RetCode_t::RETCODE_ERROR, "Error encoding rtps message.");
                    return;
                }

                msgToSend = encrypt_msg_;
            }
#endif  // if HAVE_SECURITY

            if (!sender_->send(msgToSend)) {
                return;
            }

            current_sent_bytes_ += msgToSend->length;
        }
    }
}

void RTPSMessageGroup::flush_and_reset() {
    // Flush
    flush();

    current_dst_ = c_GuidPrefix_Unknown;
}

inline bool RTPSMessageGroup::check_and_maybe_flush(const GuidPrefix_t& destination_guid_prefix,
                                                    bool is_big_submessage) {
    assert(nullptr != sender_);
    bool has_cached_pkg = full_msg_->last_pos != RTPSMESSAGE_HEADER_SIZE;
    if (full_msg_->length > full_msg_->max_size / 2 && has_cached_pkg) {
        uint32_t last_pos = full_msg_->last_pos;
        uint32_t last_length = full_msg_->pos - full_msg_->last_pos;
        full_msg_->pos = full_msg_->last_pos;
        full_msg_->length = full_msg_->last_pos;
        flush_and_reset();
        (void)add_info_dst_in_buffer(full_msg_, destination_guid_prefix);
        if (full_msg_->length + last_length > full_msg_->max_size / 2) {
            elogWarning(RTPS_WRITER, "Cannot add RTPS submesage to the CDRMessage. Buffer too small, last_length "
                                         << last_length << " len " << full_msg_->length << " max "
                                         << full_msg_->max_size / 2);
        }
        memmove(&full_msg_->buffer[full_msg_->pos], &full_msg_->buffer[last_pos], last_length);
        full_msg_->pos += last_length;
        full_msg_->length += last_length;
    }

    if (is_big_submessage) {
        flush_and_reset();
    }
    full_msg_->last_pos = full_msg_->pos;
    return true;
}

bool RTPSMessageGroup::add_info_dst_in_buffer(CDRMessage_t* buffer, const GuidPrefix_t& destination_guid_prefix) {
#if HAVE_SECURITY
    if (add_info_src_) {
        (void)RTPSMessageCreator::addSubmessageInfoSRC(buffer, c_ProtocolVersion, c_VendorId_vbs, src_guid_prefix_);
    }
    // Add INFO_SRC when we are at the beginning of the message and RTPS protection is enabled
    else if ((full_msg_->length == RTPSMESSAGE_HEADER_SIZE) && participant_->security_attributes().is_rtps_protected &&
             endpoint_->supports_rtps_protection()) {
        (void)RTPSMessageCreator::addSubmessageInfoSRC(buffer, c_ProtocolVersion, c_VendorId_vbs,
                                                       participant_->getGuid().guidPrefix);
    }
#else
    if (add_info_src_) {
        (void)RTPSMessageCreator::addSubmessageInfoSRC(buffer, c_ProtocolVersion, c_VendorId_vbs, src_guid_prefix_);
    }
#endif  // if HAVE_SECURITY
    if (current_dst_ != destination_guid_prefix) {
        current_dst_ = destination_guid_prefix;
        (void)RTPSMessageCreator::addSubmessageInfoDST(buffer, current_dst_);
    }

    return true;
}

bool RTPSMessageGroup::add_info_ts_in_buffer(const Time_t& timestamp) {
    assert(nullptr != sender_);

#if HAVE_SECURITY
    uint32_t from_buffer_position = full_msg_->pos;
#endif  // if HAVE_SECURITY

    if (!RTPSMessageCreator::addSubmessageInfoTS(full_msg_, timestamp, false)) {
        elogError(RTPS_WRITER, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                  "Cannot add INFO_TS submsg to the CDRMessage. Buffer too small");
        return false;
    }

#if HAVE_SECURITY
    if (endpoint_->getAttributes().security_attributes().is_submessage_protected) {
        full_msg_->pos = from_buffer_position;
        (void)CDRMessage::initCDRMsg(encrypt_msg_);
        if (!participant_->security_manager().encode_writer_submessage(*full_msg_, *encrypt_msg_, endpoint_->getGuid(),
                                                                       sender_->remote_guids())) {
            elogError(RTPS_WRITER, RetCode_t::RETCODE_ERROR,
                      "Cannot encrypt DATA submessage for writer " << endpoint_->getGuid());
            return false;
        }

        if ((full_msg_->max_size - from_buffer_position) >= encrypt_msg_->length) {
            (void)memcpy(&full_msg_->buffer[from_buffer_position], encrypt_msg_->buffer, encrypt_msg_->length);
            full_msg_->length = from_buffer_position + encrypt_msg_->length;
            full_msg_->pos = full_msg_->length;
        } else {
            elogError(RTPS_MSG_OUT, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                      "Not enough memory to copy encrypted data for " << endpoint_->getGuid());
            return false;
        }
    }
#endif  // if HAVE_SECURITY

    return true;
}

#if HAVE_SECURITY
bool RTPSMessageGroup::add_auth_token_in_buffer(const GuidPrefix_t& dst, uint32_t token) {
    if (!participant_->security_manager().authTokenAlwaysOn && !participant_->is_secure()) {
        return true;
    }

    if (token == 0 || endpoint_->getGuid().entityId == c_EntityId_SPDPWriter || dst.value[0] != c_VendorId_vbs[0]) {
        logDebug(AUTHTOKEN, "send authtoken from source:" << endpoint_->getGuid() << " to destination:" << dst);
        return true;
    }

    uint32_t from_buffer_position = full_msg_->pos;

    if (!RTPSMessageCreator::addSubmessageAuthToken(full_msg_, token)) {
        elogError(RTPS_WRITER, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                  "Cannot add AUTH_TOKEN submsg to the CDRMessage. Buffer too small");
        return false;
    }

    if (endpoint_->getAttributes().security_attributes().is_submessage_protected) {
        full_msg_->pos = from_buffer_position;
        (void)CDRMessage::initCDRMsg(encrypt_msg_);
        if (endpoint_->getGuid().entityId.is_writer()) {
            if (!participant_->security_manager().encode_writer_submessage(
                    *full_msg_, *encrypt_msg_, endpoint_->getGuid(), sender_->remote_guids())) {
                elogError(RTPS_WRITER, RetCode_t::RETCODE_ERROR,
                          "Cannot encrypt AUTHTOKEN submessage for writer " << endpoint_->getGuid());
                return false;
            }
        } else {
            if (!participant_->security_manager().encode_reader_submessage(
                    *full_msg_, *encrypt_msg_, endpoint_->getGuid(), sender_->remote_guids())) {
                elogError(RTPS_READER, RetCode_t::RETCODE_ERROR,
                          "Cannot encrypt AUTHTOKEN submessage for writer " << endpoint_->getGuid());
                return false;
            }
        }

        if ((full_msg_->max_size - from_buffer_position) >= encrypt_msg_->length) {
            (void)memcpy(&full_msg_->buffer[from_buffer_position], encrypt_msg_->buffer, encrypt_msg_->length);
            full_msg_->length = from_buffer_position + encrypt_msg_->length;
            full_msg_->pos = full_msg_->length;
        } else {
            elogError(RTPS_MSG_OUT, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                      "Not enough memory to copy encrypted data for " << endpoint_->getGuid());
            return false;
        }
    }

    return true;
}

std::vector<uint8_t> RTPSMessageGroup::key_derivation(const std::vector<uint8_t>& key, const GuidPrefix_t& src,
                                                      const GuidPrefix_t& dst) {
    uint32_t guid_size = GuidPrefix_t::size + EntityId_t::size;
    std::vector<uint8_t> data(guid_size * 2, 0);

    if (src < dst) {
        std::memcpy(data.data(), src.value, GuidPrefix_t::size);
        std::memcpy(data.data() + guid_size, dst.value, GuidPrefix_t::size);
    } else {
        std::memcpy(data.data(), dst.value, GuidPrefix_t::size);
        std::memcpy(data.data() + guid_size, src.value, GuidPrefix_t::size);
    }

    std::vector<uint8_t> result(SHA256_DIGEST_LENGTH, 0);
    unsigned int result_len = 0;

    HMAC(EVP_sha256(), key.data(), key.size(), data.data(), data.size(), result.data(), &result_len);

    return result;
}

bool RTPSMessageGroup::add_shared_key_in_buffer(const std::vector<uint8_t>& shared_key, const GuidPrefix_t& matched_src,
                                                const GuidPrefix_t& matched_dst) {
    if (!participant_->is_secure()) {
        return true;
    }

    VendorId_t vid;
    matched_dst.get_vendor_id(vid);
    if (endpoint_->getGuid().entityId == c_EntityId_SPDPWriter || vid != c_VendorId_vbs) {
        return true;
    }

    if (shared_key.empty() || participant_->security_manager().is_client ||
        (endpoint_->getGuid().entityId != c_EntityId_SEDPPubWriter &&
         endpoint_->getGuid().entityId != c_EntityId_SEDPSubWriter &&
         endpoint_->getGuid().entityId != sedp_builtin_publications_secure_writer &&
         endpoint_->getGuid().entityId != sedp_builtin_subscriptions_secure_writer)) {
        return true;
    }

    const std::vector<uint8_t> derived_key = (endpoint_->getGuid().get_host_id() == matched_dst.get_host_id())
                                                 ? key_derivation(shared_key, matched_src, matched_dst)
                                                 : shared_key;

    uint32_t from_buffer_position = full_msg_->pos;

    if (!RTPSMessageCreator::addSubmessageSharedKey(full_msg_, derived_key)) {
        elogError(RTPS_WRITER, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                  "Cannot add AUTH_TOKEN submsg to the CDRMessage. Buffer too small");
        return false;
    }

    if (endpoint_->getAttributes().security_attributes().is_submessage_protected) {
        full_msg_->pos = from_buffer_position;
        (void)CDRMessage::initCDRMsg(encrypt_msg_);
        if (endpoint_->getGuid().entityId.is_writer()) {
            if (!participant_->security_manager().encode_writer_submessage(
                    *full_msg_, *encrypt_msg_, endpoint_->getGuid(), sender_->remote_guids())) {
                elogError(RTPS_WRITER, RetCode_t::RETCODE_ERROR,
                          "Cannot encrypt AUTHTOKEN submessage for writer " << endpoint_->getGuid());
                return false;
            }
        } else {
            if (!participant_->security_manager().encode_reader_submessage(
                    *full_msg_, *encrypt_msg_, endpoint_->getGuid(), sender_->remote_guids())) {
                elogError(RTPS_READER, RetCode_t::RETCODE_ERROR,
                          "Cannot encrypt AUTHTOKEN submessage for writer " << endpoint_->getGuid());
                return false;
            }
        }

        if ((full_msg_->max_size - from_buffer_position) >= encrypt_msg_->length) {
            (void)memcpy(&full_msg_->buffer[from_buffer_position], encrypt_msg_->buffer, encrypt_msg_->length);
            full_msg_->length = from_buffer_position + encrypt_msg_->length;
            full_msg_->pos = full_msg_->length;
        } else {
            elogError(RTPS_MSG_OUT, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                      "Not enough memory to copy encrypted data for " << endpoint_->getGuid());
            return false;
        }
    }

    return true;
}

void RTPSMessageGroup::get_authtoken(Endpoint* endpoint, uint64_t seq) {
    const GuidPrefix_t& src = endpoint->getGuid().guidPrefix;
    const GuidPrefix_t& dst = sender_->destination_guid_prefix();

    m_token = participant_->security_manager().get_authtoken(src, dst, seq);
}

#endif

bool RTPSMessageGroup::add_info_e2e_in_buffer(const EntityId_t& readerId, CacheChange_t& change) {
    assert(nullptr != sender_);

#if HAVE_SECURITY
    uint32_t from_buffer_position = full_msg_->pos;
#endif  // if HAVE_SECURITY

    if (!RTPSMessageCreator::addSubmessageInfoE2E(full_msg_, readerId, change)) {
        elogError(RTPS_WRITER, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                  "Cannot add INFO_E2E submsg to the CDRMessage. Buffer too small");
        return false;
    }

#if HAVE_SECURITY
    if (endpoint_->getAttributes().security_attributes().is_submessage_protected) {
        full_msg_->pos = from_buffer_position;
        (void)CDRMessage::initCDRMsg(encrypt_msg_);
        if (!participant_->security_manager().encode_writer_submessage(*full_msg_, *encrypt_msg_, endpoint_->getGuid(),
                                                                       sender_->remote_guids())) {
            elogError(RTPS_WRITER, RetCode_t::RETCODE_ERROR,
                      "Cannot encrypt DATA submessage for writer " << endpoint_->getGuid());
            return false;
        }

        if ((full_msg_->max_size - from_buffer_position) >= encrypt_msg_->length) {
            (void)memcpy(&full_msg_->buffer[from_buffer_position], encrypt_msg_->buffer,
                         static_cast<uint64_t>(encrypt_msg_->length));
            full_msg_->length = from_buffer_position + encrypt_msg_->length;
            full_msg_->pos = full_msg_->length;
        } else {
            elogError(RTPS_MSG_OUT, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                      "Not enough memory to copy encrypted data for " << endpoint_->getGuid());
            return false;
        }
    }
#endif  // if HAVE_SECURITY

    return true;
}

bool RTPSMessageGroup::add_data(CacheChange_t& change, bool enable_e2e, /*uint32_t dataId_e2e,*/
                                E2EProfile04_t& e2e_profile04) {
    assert(nullptr != sender_);

    UNUSED_PARAMETER(e2e_profile04);
    // Check limitation
    uint32_t data_size = change.serializedPayload.length;
    if (data_exceeds_limitation(data_size, sent_bytes_limitation_, current_sent_bytes_, full_msg_->length)) {
        //flush_and_reset();
        flush_and_reset();
        throw limit_exceeded();
    }

#if HAVE_SECURITY
    get_authtoken(endpoint_, change.sequenceNumber.to64long());
    (void)add_auth_token_in_buffer(sender_->destination_guid_prefix(), m_token);
#endif
    (void)add_info_dst_in_buffer(full_msg_, sender_->destination_guid_prefix());
    (void)add_info_ts_in_buffer(change.sourceTimestamp);

    CacheChangeInlineQoSWriter qos_writer(change);
    InlineQosWriter* inline_qos;
    inline_qos = (change.inline_qos.length > 0 && nullptr != change.inline_qos.data) ? &qos_writer : nullptr;

#if HAVE_SECURITY
    uint32_t from_buffer_position = full_msg_->pos;
#endif  // if HAVE_SECURITY
    const EntityId_t& readerId = get_entity_id(sender_->remote_guids());

    CacheChange_t change_to_add;
    change_to_add.copy_not_memcpy(&change);
    change_to_add.serializedPayload.data = change.serializedPayload.data;
    change_to_add.serializedPayload.origin_data = change.serializedPayload.origin_data;
    change_to_add.serializedPayload.length = change.serializedPayload.length;
    change_to_add.nonSerializedPayload.data = change.nonSerializedPayload.data;
    change_to_add.nonSerializedPayload.length = change.nonSerializedPayload.length;
    change_to_add.writerGUID = endpoint_->getGuid();

#if HAVE_SECURITY
    if (endpoint_->getAttributes().security_attributes().is_payload_protected) {
        SerializedPayload_t encrypt_payload;
        encrypt_payload.data = encrypt_msg_->buffer;
        encrypt_payload.max_size = encrypt_msg_->max_size;

        // If payload protection, encode payload
        if (!participant_->security_manager().encode_serialized_payload(change_to_add.serializedPayload,
                                                                        encrypt_payload, endpoint_->getGuid())) {
            elogError(RTPS_WRITER, RetCode_t::RETCODE_ERROR, "Error encoding change " << change.sequenceNumber);
            change_to_add.serializedPayload.data = nullptr;
            encrypt_payload.data = nullptr;
            return false;
        }

        change_to_add.serializedPayload.data = encrypt_msg_->buffer;
        change_to_add.serializedPayload.origin_data = nullptr;
        encrypt_payload.data = nullptr;
        change_to_add.serializedPayload.length = encrypt_payload.length;
    }
#endif  // if HAVE_SECURITY

    if (enable_e2e) {
        (void)add_info_e2e_in_buffer(readerId, change_to_add /*, e2e_profile04, dataId_e2e*/);
        change.e2eCounter = change_to_add.e2eCounter;
    }
    // TODO (Ricardo). Check to create special wrapper.
    bool is_big_submessage;
    if (!RTPSMessageCreator::addSubmessageData(full_msg_, &change_to_add, endpoint_->getAttributes().topicKind,
                                               readerId, inline_qos, &is_big_submessage)) {
        elogErrorT(RTPS_WRITER, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                   "Cannot add DATA submsg to the CDRMessage. Buffer too small");
        change_to_add.serializedPayload.data = nullptr;
        change_to_add.nonSerializedPayload.data = nullptr;
        return false;
    }
    change_to_add.serializedPayload.data = nullptr;
    change_to_add.nonSerializedPayload.data = nullptr;
#if !defined(WIN32)
    if (change.trace_point_status == vbsutil::tracepoint::ON_EXPORT) {
        auto& trace_point_info = vbsutil::tracepoint::TracePointManager::get_trace_point_info();
        if (trace_point_info.trace_point_status == vbsutil::tracepoint::INVALID) {
            trace_point_info.trace_point_status = change.trace_point_status;
            trace_point_info.topic_name = change.topic_name;
            trace_point_info.vbs_version = change.vbs_version;
            trace_point_info.domain_id = participant_->get_domain_id();
        }

        vbsutil::tracepoint::TracePointInfo::DataInfo tmp_info;
        tmp_info.seq_num = static_cast<uint32_t>(change_to_add.sequenceNumber.to64long());
        tmp_info.offset = change_to_add.serialized_data_offset;
        tmp_info.length = change_to_add.serializedPayload.length;
        trace_point_info.data_infos.emplace_back(tmp_info);
    }
#endif
#if HAVE_SECURITY
    if (endpoint_->getAttributes().security_attributes().is_submessage_protected) {
        full_msg_->pos = from_buffer_position;
        (void)CDRMessage::initCDRMsg(encrypt_msg_);
        if (!participant_->security_manager().encode_writer_submessage(*full_msg_, *encrypt_msg_, endpoint_->getGuid(),
                                                                       sender_->remote_guids())) {
            elogError(RTPS_WRITER, RetCode_t::RETCODE_ERROR,
                      "Cannot encrypt DATA submessage for writer " << endpoint_->getGuid());
            return false;
        }

        if ((full_msg_->max_size - from_buffer_position) >= encrypt_msg_->length) {
            (void)memcpy(&full_msg_->buffer[from_buffer_position], encrypt_msg_->buffer,
                         static_cast<uint64_t>(encrypt_msg_->length));
            full_msg_->length = from_buffer_position + encrypt_msg_->length;
            full_msg_->pos = full_msg_->length;
        } else {
            elogError(RTPS_MSG_OUT, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                      "Not enough memory to copy encrypted data for " << endpoint_->getGuid());
            return false;
        }
    }
    auto key = participant_->security_manager().get_shared_key();
    if (key) {
        (void)add_shared_key_in_buffer(*key, static_cast<const GUID_t&>(change.instanceHandle).guidPrefix,
                                       sender_->destination_guid_prefix());
    }
#endif  // if HAVE_SECURITY

    return check_and_maybe_flush(sender_->destination_guid_prefix(), is_big_submessage);
}

bool RTPSMessageGroup::add_data_frag(CacheChange_t& change, const uint32_t fragment_number, bool enable_e2e,
                                     E2EProfile04_t& e2e_profile04) {
    assert(nullptr != sender_);

    UNUSED_PARAMETER(e2e_profile04);
    // Calculate fragment start
    uint32_t fragment_start = change.getFragmentSize() * (fragment_number - 1U);
    // Calculate fragment size. If last fragment, size may be smaller
    uint32_t fragment_size =
        fragment_number < change.getFragmentCount()
            ? change.getFragmentSize()
            : (change.serializedPayload.length + change.nonSerializedPayload.length - fragment_start);
    // Check limitation
    if (data_exceeds_limitation(fragment_size, sent_bytes_limitation_, current_sent_bytes_, full_msg_->length)) {
        //flush_and_reset();
        flush_and_reset();
        throw limit_exceeded();
    }

    // Check preconditions. If fail flush and reset.
#if HAVE_SECURITY
    get_authtoken(endpoint_, change.sequenceNumber.to64long());
    (void)add_auth_token_in_buffer(sender_->destination_guid_prefix(), m_token);
#endif
    (void)add_info_dst_in_buffer(full_msg_, sender_->destination_guid_prefix());
    (void)add_info_ts_in_buffer(change.sourceTimestamp);

    CacheChangeInlineQoSWriter qos_writer(change);
    InlineQosWriter* inline_qos;
    inline_qos = (change.inline_qos.length > 0 && nullptr != change.inline_qos.data) ? &qos_writer : nullptr;
#if HAVE_SECURITY
    uint32_t from_buffer_position = full_msg_->pos;
#endif  // if HAVE_SECURITY
    const EntityId_t& readerId = get_entity_id(sender_->remote_guids());

    // TODO (Ricardo). Check to create special wrapper.
    CacheChange_t change_to_add;
    change_to_add.copy_not_memcpy(&change);
    if (change.nonSerializedPayload.data && (fragment_start > change.serializedPayload.length)) {
        size_t offset = fragment_start - change.serializedPayload.length;
        change_to_add.serializedPayload.data = change.nonSerializedPayload.data + offset;
        change_to_add.serializedPayload.length = fragment_size;
    } else if (change.nonSerializedPayload.data &&
               (change.serializedPayload.length < (fragment_start + fragment_size))) {
        change_to_add.serializedPayload.data = change.serializedPayload.data + fragment_start;
        change_to_add.serializedPayload.length = change.serializedPayload.length - fragment_start;
        change_to_add.nonSerializedPayload.data = change.nonSerializedPayload.data;
        change_to_add.nonSerializedPayload.length = fragment_size - (change.serializedPayload.length - fragment_start);
    } else {
        if (change.serializedPayload.origin_data) {
            if (fragment_start == 0) {
                change_to_add.serializedPayload.origin_data = change.serializedPayload.origin_data;
            } else {
                change_to_add.serializedPayload.origin_data = change.serializedPayload.origin_data + fragment_start -
                                                              change.serializedPayload.representation_header_size;
            }
        }
        change_to_add.serializedPayload.data = change.serializedPayload.data + fragment_start;
        change_to_add.serializedPayload.length = fragment_size;
    }
    change_to_add.writerGUID = endpoint_->getGuid();

#if HAVE_SECURITY
    if (endpoint_->getAttributes().security_attributes().is_payload_protected) {
        SerializedPayload_t encrypt_payload;
        encrypt_payload.data = encrypt_msg_->buffer;
        encrypt_payload.max_size = encrypt_msg_->max_size;

        // If payload protection, encode payload
        if (!participant_->security_manager().encode_serialized_payload(change_to_add.serializedPayload,
                                                                        encrypt_payload, endpoint_->getGuid())) {
            elogError(RTPS_WRITER, RetCode_t::RETCODE_ERROR, "Error encoding change " << change.sequenceNumber);
            change_to_add.serializedPayload.data = nullptr;
            encrypt_payload.data = nullptr;
            return false;
        }

        change_to_add.serializedPayload.data = encrypt_msg_->buffer;
        encrypt_payload.data = nullptr;
        change_to_add.serializedPayload.length = encrypt_payload.length;
    }
#endif  // if HAVE_SECURITY

    if (enable_e2e && (fragment_number == change.getFragmentCount())) {
        (void)add_info_e2e_in_buffer(readerId, change);
        change.e2eCounter = change_to_add.e2eCounter;
    }

    if (!RTPSMessageCreator::addSubmessageDataFrag(full_msg_, &change, fragment_number, change_to_add,
                                                   endpoint_->getAttributes().topicKind, readerId, inline_qos)) {
        elogErrorT(RTPS_WRITER, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                   "Cannot add DATA_FRAG submsg to the CDRMessage. Buffer too small");
        change_to_add.serializedPayload.data = nullptr;
        change_to_add.nonSerializedPayload.data = nullptr;
        return false;
    }
    change_to_add.serializedPayload.data = nullptr;
    change_to_add.nonSerializedPayload.data = nullptr;

#if HAVE_SECURITY
    if (endpoint_->getAttributes().security_attributes().is_submessage_protected) {
        full_msg_->pos = from_buffer_position;
        (void)CDRMessage::initCDRMsg(encrypt_msg_);
        if (!participant_->security_manager().encode_writer_submessage(*full_msg_, *encrypt_msg_, endpoint_->getGuid(),
                                                                       sender_->remote_guids())) {
            elogError(RTPS_WRITER, RetCode_t::RETCODE_ERROR,
                      "Cannot encrypt DATA submessage for writer " << endpoint_->getGuid());
            return false;
        }

        if ((full_msg_->max_size - from_buffer_position) >= encrypt_msg_->length) {
            (void)memcpy(&full_msg_->buffer[from_buffer_position], encrypt_msg_->buffer,
                         static_cast<uint64_t>(encrypt_msg_->length));
            full_msg_->length = from_buffer_position + encrypt_msg_->length;
            full_msg_->pos = full_msg_->length;
        } else {
            elogError(RTPS_MSG_OUT, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                      "Not enough memory to copy encrypted data for " << endpoint_->getGuid());
            return false;
        }
    }
#endif  // if HAVE_SECURITY

    return check_and_maybe_flush(sender_->destination_guid_prefix(), false);
}

bool RTPSMessageGroup::add_heartbeat(const SequenceNumber_t& firstSN, const SequenceNumber_t& lastSN,
                                     const Count_t count, bool isFinal, bool livelinessFlag) {
    assert(nullptr != sender_);

#if HAVE_SECURITY
    get_authtoken(endpoint_, count);
    (void)add_auth_token_in_buffer(sender_->destination_guid_prefix(), m_token);
#endif
    (void)add_info_dst_in_buffer(full_msg_, sender_->destination_guid_prefix());

#if HAVE_SECURITY
    uint32_t from_buffer_position = full_msg_->pos;
#endif  // if HAVE_SECURITY

    const EntityId_t& readerId = get_entity_id(sender_->remote_guids());

    if (!RTPSMessageCreator::addSubmessageHeartbeat(full_msg_, readerId, endpoint_->getGuid().entityId, firstSN, lastSN,
                                                    count, isFinal, livelinessFlag)) {
        elogError(RTPS_WRITER, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                  "Cannot add HEARTBEAT submsg to the CDRMessage. Buffer too small");
        return false;
    }

#if HAVE_SECURITY
    if (endpoint_->getAttributes().security_attributes().is_submessage_protected) {
        full_msg_->pos = from_buffer_position;
        (void)CDRMessage::initCDRMsg(encrypt_msg_);
        if (!participant_->security_manager().encode_writer_submessage(*full_msg_, *encrypt_msg_, endpoint_->getGuid(),
                                                                       sender_->remote_guids())) {
            elogError(RTPS_WRITER, RetCode_t::RETCODE_ERROR,
                      "Cannot encrypt HEARTBEAT submessage for writer " << endpoint_->getGuid());
            return false;
        }

        if ((full_msg_->max_size - from_buffer_position) >= encrypt_msg_->length) {
            (void)memcpy(&full_msg_->buffer[from_buffer_position], encrypt_msg_->buffer, encrypt_msg_->length);
            full_msg_->length = from_buffer_position + encrypt_msg_->length;
            full_msg_->pos = full_msg_->length;
        } else {
            elogError(RTPS_MSG_OUT, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                      "Not enough memory to copy encrypted data for " << endpoint_->getGuid());
            return false;
        }
    }
#endif  // if HAVE_SECURITY

    return check_and_maybe_flush(sender_->destination_guid_prefix(), false);
}

// TODO (Ricardo) Check with standard 8.3.7.4.5
bool RTPSMessageGroup::add_gap(std::set<SequenceNumber_t>& changesSeqNum) {
    RTPSGapBuilder gap_builder(*this);
    for (const SequenceNumber_t& seq : changesSeqNum) {
        if (!gap_builder.add(seq)) {
            return false;
        }
    }

    return gap_builder.flush();
}

bool RTPSMessageGroup::add_gap(const SequenceNumber_t& gap_initial_sequence, const SequenceNumberSet_t& gap_bitmap) {
    assert(nullptr != sender_);

    // Check preconditions. If fail flush and reset.
#if HAVE_SECURITY
    get_authtoken(endpoint_, endpoint_->m_auth_token.max_blocks_per_session + 1);
    (void)add_auth_token_in_buffer(sender_->destination_guid_prefix(), m_token);
#endif
    (void)add_info_dst_in_buffer(full_msg_, sender_->destination_guid_prefix());

    const EntityId_t& readerId = get_entity_id(sender_->remote_guids());

    if (!create_gap_submessage(gap_initial_sequence, gap_bitmap, readerId)) {
        return false;
    }

    return check_and_maybe_flush(sender_->destination_guid_prefix(), false);
}

bool RTPSMessageGroup::add_gap(const SequenceNumber_t& gap_initial_sequence, const SequenceNumberSet_t& gap_bitmap,
                               const GUID_t& reader_guid) {
    // Check preconditions. If fail flush and reset.
#if HAVE_SECURITY
    get_authtoken(endpoint_, endpoint_->m_auth_token.max_blocks_per_session + 1);
    (void)add_auth_token_in_buffer(sender_->destination_guid_prefix(), m_token);
#endif
    (void)add_info_dst_in_buffer(full_msg_, sender_->destination_guid_prefix());

    const EntityId_t& readerId = reader_guid.entityId;

    if (!create_gap_submessage(gap_initial_sequence, gap_bitmap, readerId)) {
        return false;
    }

    return check_and_maybe_flush(reader_guid.guidPrefix, false);
}

bool RTPSMessageGroup::create_gap_submessage(const SequenceNumber_t& gap_initial_sequence,
                                             const SequenceNumberSet_t& gap_bitmap, const EntityId_t& reader_id) {
    assert(nullptr != sender_);

#if HAVE_SECURITY
    uint32_t from_buffer_position = full_msg_->pos;
#endif  // if HAVE_SECURITY

    if (!RTPSMessageCreator::addSubmessageGap(full_msg_, gap_initial_sequence, gap_bitmap, reader_id,
                                              endpoint_->getGuid().entityId)) {
        elogError(RTPS_WRITER, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                  "Cannot add GAP submsg to the CDRMessage. Buffer too small");
        return false;
    }

#if HAVE_SECURITY
    if (endpoint_->getAttributes().security_attributes().is_submessage_protected) {
        full_msg_->pos = from_buffer_position;
        (void)CDRMessage::initCDRMsg(encrypt_msg_);
        if (!participant_->security_manager().encode_writer_submessage(*full_msg_, *encrypt_msg_, endpoint_->getGuid(),
                                                                       sender_->remote_guids())) {
            elogError(RTPS_WRITER, RetCode_t::RETCODE_ERROR,
                      "Cannot encrypt DATA submessage for writer " << endpoint_->getGuid());
            return false;
        }

        if ((full_msg_->max_size - from_buffer_position) >= encrypt_msg_->length) {
            (void)memcpy(&full_msg_->buffer[from_buffer_position], encrypt_msg_->buffer, encrypt_msg_->length);
            full_msg_->length = from_buffer_position + encrypt_msg_->length;
            full_msg_->pos = full_msg_->length;
        } else {
            elogError(RTPS_MSG_OUT, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                      "Not enough memory to copy encrypted data for " << endpoint_->getGuid());
            return false;
        }
    }
#endif  // if HAVE_SECURITY
    return true;
}

bool RTPSMessageGroup::add_acknack(const SequenceNumberSet_t& SNSet, int32_t count, bool finalFlag) {
    assert(nullptr != sender_);

    // A vector is used to avoid dynamic allocations, but only first item is used
    size_t n_guids = sender_->remote_guids().size();
    if (n_guids == 0U) {
        return false;
    }
    assert(n_guids == 1);

#if HAVE_SECURITY
    get_authtoken(endpoint_, count);
    (void)add_auth_token_in_buffer(sender_->destination_guid_prefix(), m_token);
#endif
    (void)add_info_dst_in_buffer(full_msg_, sender_->destination_guid_prefix());

#if HAVE_SECURITY
    uint32_t from_buffer_position = full_msg_->pos;
#endif  // if HAVE_SECURITY

    if (!RTPSMessageCreator::addSubmessageAcknack(full_msg_, endpoint_->getGuid().entityId,
                                                  sender_->remote_guids().front().entityId, SNSet, count, finalFlag)) {
        elogError(RTPS_READER, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                  "Cannot add ACKNACK submsg to the CDRMessage. Buffer too small");
        return false;
    }

#if HAVE_SECURITY
    if (endpoint_->getAttributes().security_attributes().is_submessage_protected) {
        full_msg_->pos = from_buffer_position;
        (void)CDRMessage::initCDRMsg(encrypt_msg_);
        if (!participant_->security_manager().encode_reader_submessage(*full_msg_, *encrypt_msg_, endpoint_->getGuid(),
                                                                       sender_->remote_guids())) {
            elogError(RTPS_READER, RetCode_t::RETCODE_ERROR,
                      "Cannot encrypt ACKNACK submessage for writer " << endpoint_->getGuid());
            return false;
        }

        if ((full_msg_->max_size - from_buffer_position) >= encrypt_msg_->length) {
            (void)memcpy(&full_msg_->buffer[from_buffer_position], encrypt_msg_->buffer, encrypt_msg_->length);
            full_msg_->length = from_buffer_position + encrypt_msg_->length;
            full_msg_->pos = full_msg_->length;
        } else {
            elogError(RTPS_MSG_OUT, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                      "Not enough memory to copy encrypted data for " << endpoint_->getGuid());
            return false;
        }
    }
#endif  // if HAVE_SECURITY
    return check_and_maybe_flush(sender_->destination_guid_prefix(), false);
}

bool RTPSMessageGroup::add_nackfrag(const SequenceNumber_t& writerSN, FragmentNumberSet_t fnState, int32_t count) {
    assert(nullptr != sender_);

    // A vector is used to avoid dynamic allocations, but only first item is used
    assert(sender_->remote_guids().size() == 1);

#if HAVE_SECURITY
    get_authtoken(endpoint_, count);
    (void)add_auth_token_in_buffer(sender_->destination_guid_prefix(), m_token);
#endif
    (void)add_info_dst_in_buffer(full_msg_, sender_->destination_guid_prefix());

#if HAVE_SECURITY
    uint32_t from_buffer_position = full_msg_->pos;
#endif  // if HAVE_SECURITY

    if (!RTPSMessageCreator::addSubmessageNackFrag(full_msg_, endpoint_->getGuid().entityId,
                                                   sender_->remote_guids().front().entityId, writerSN, fnState,
                                                   count)) {
        elogError(RTPS_READER, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                  "Cannot add ACKNACK submsg to the CDRMessage. Buffer too small");
        return false;
    }

#if HAVE_SECURITY
    if (endpoint_->getAttributes().security_attributes().is_submessage_protected) {
        full_msg_->pos = from_buffer_position;
        (void)CDRMessage::initCDRMsg(encrypt_msg_);
        if (!participant_->security_manager().encode_reader_submessage(*full_msg_, *encrypt_msg_, endpoint_->getGuid(),
                                                                       sender_->remote_guids())) {
            elogError(RTPS_READER, RetCode_t::RETCODE_ERROR,
                      "Cannot encrypt ACKNACK submessage for writer " << endpoint_->getGuid());
            return false;
        }

        if ((full_msg_->max_size - from_buffer_position) >= encrypt_msg_->length) {
            (void)memcpy(&full_msg_->buffer[from_buffer_position], encrypt_msg_->buffer, encrypt_msg_->length);
            full_msg_->length = from_buffer_position + encrypt_msg_->length;
            full_msg_->pos = full_msg_->length;
        } else {
            elogError(RTPS_MSG_OUT, RetCode_t::RETCODE_OUT_OF_RESOURCES,
                      "Not enough memory to copy encrypted data for " << endpoint_->getGuid());
            return false;
        }
    }
#endif  // if HAVE_SECURITY
    return check_and_maybe_flush(sender_->destination_guid_prefix(), false);
}

} /* namespace rtps */
} /* namespace ertps */
} /* namespace evbs */
